// 
// File:   test_model.cc
// Author: cionescu
//
// Created on February 5, 2007, 3:09 PM
//
#include <util.h>
#include <cudd.h>
#include "dd/dd.h"
#include "add_matrix.h"
#include "add_vector.h"
#include "add_variables.h"
#include <math.h>
#include <vector>

//using std::cout;
//using std::endl;
DdNode*
reachability(DdManager* m, DdNode* p, DdNode* init, DdNode** rows, DdNode** cols, int num) {
  bool done = false;
  DdNode* tmp;
  int i =0;
  
  // transpose ?!
  DdNode* reach = DD_PermuteVariables(m, init, rows, cols, num);
  Cudd_Ref(init);
  
  printf("start reachability\n");
  DD_PrintVector(m, reach, cols, num);
  while(!done) {
    printf("iteration number %d\n", i++);
    Cudd_Ref(reach);
    tmp = DD_PermuteVariables(m, reach, cols, rows, num);
    Cudd_Ref(p);
    tmp = DD_And(m, tmp, p);
    tmp = DD_ThereExists(m, tmp, rows, num);
    Cudd_Ref(reach);
    tmp = DD_Or(m, reach, tmp);
    DD_PrintVector(m, tmp, cols, num);
    if (tmp == reach) {
            done = true;
    }
    Cudd_RecursiveDeref(m, reach);
    reach = tmp;
  }
  reach = DD_PermuteVariables(m, reach, cols, rows, num);
  printf("\nend reachability\n");
  return reach;
}

DdNode*
restrict(DdManager* manager, DdNode* matrix, DdNode* reach, DdNode** cols, DdNode** rows, int num) {
    Cudd_Ref(matrix);
    matrix = DD_Apply(manager, APPLY_TIMES, reach, matrix);
    reach = DD_PermuteVariables(manager, reach, cols, rows, num);
    Cudd_Ref(matrix);
    matrix = DD_Apply(manager, APPLY_TIMES, reach, matrix);
    Cudd_Ref(matrix);
    return matrix;
}
enum {
  TERM_CRIT_ABSOLUTE, TERM_CRIT_RELATIVE
} ;

DdNode*
solve(DdManager* manager, DdNode* m, DdNode* init, DdNode** rows, int rnum, DdNode** cols, int cnum, DdNode* b) {
  int term_crit = TERM_CRIT_ABSOLUTE;
  int iters = 0, max_iters = 100;
  bool done = false;
  DdNode *tmp, *sol;
  
  sol = init;
  printf("solve computation\n");
  while (!done && iters < max_iters) {

    iters++;

    // matrix multiply
    Cudd_Ref(sol);
    tmp = DD_PermuteVariables(manager, sol, rows, cols, rnum);
    Cudd_Ref(m);
    tmp = DD_MatrixMultiply(manager, m, tmp, rows, cnum, MM_BOULDER);
    Cudd_Ref(b);
    tmp = DD_Apply(manager, APPLY_PLUS, tmp, b);

    // check convergence
    switch (term_crit) {
      case TERM_CRIT_ABSOLUTE:
        if (DD_EqualSupNorm(manager, tmp, sol, 0.00001)) {
                done = true;
        }
        break;
      case TERM_CRIT_RELATIVE:
        if (DD_EqualSupNormRel(manager, tmp, sol, 0.01)) {
                done = true;
        }
        break;
    }

        // prepare for next iteration
    Cudd_RecursiveDeref(manager, sol);
    sol = tmp;
    DD_PrintVector(manager, sol, rows, rnum );	
    }

  return sol;
}

int
main(int argc, char** argv) {    
    // simple test program for the c++ API
    // 1 module - 2 variables

    printf("test started\n");
    DdManager* manager;
    
    manager = DD_InitialiseCUDD();
    printf("CUDD manager initialized\n");
    
    add_variables cols(manager);
    cols.new_variable(2, "x");
    cols.new_variable(1, "y");
    add_variables rows(manager);
    rows.new_variable(2, "x'");
    rows.new_variable(1, "y'");
    printf("variables set up\n");
    
    add_vector v(manager, cols);
    printf("vector created\n");
    v.set_element(2, 1);
    v.print();
    printf("elements set up\n");
    
    add_matrix m(manager, rows, cols);
    printf("matrix created\n");
    for (int i = 0; i<8; i++) 
      m.set_element(i, i, .1);
    m.set_element(2, 4, 0.2);
    m.set_element(4, 5, 0.4);
    m.set_element(4, 7, 0.3);
    m.print();
    printf("matrix set up\n");
    
    // positive reachability relation
    DdNode* prr = DD_GreaterThan(manager, m.get_add(), 0);
    DD_PrintMatrix(manager, prr, rows.get_raw_vector(), rows.get_encoding_size(), 
                                  cols.get_raw_vector(), cols.get_encoding_size());
    
    // reachability
    DdNode* reach = reachability(manager, prr, v.get_add(), rows.get_raw_vector(), cols.get_raw_vector(), rows.get_encoding_size());
    DD_PrintVector(manager, reach, rows.get_raw_vector(), rows.get_encoding_size());
    printf("\n");
    
    // restriction
    DdNode* mprime = restrict(manager, m.get_add(), reach, rows.get_raw_vector(), cols.get_raw_vector(), rows.get_encoding_size());
    DD_PrintMatrix(manager, mprime, rows.get_raw_vector(), rows.get_encoding_size(), 
                                  cols.get_raw_vector(), cols.get_encoding_size());
    
    // quantification
    DdNode* b = DD_Constant(manager, 0.1);
    DdNode* prob = solve(manager, mprime, v.get_add(), rows.get_raw_vector(), rows.get_encoding_size(),
                                            cols.get_raw_vector(), cols.get_encoding_size(), b);
    DD_PrintVector(manager, prob, rows.get_raw_vector(), rows.get_encoding_size() );
    
    //DD_CloseDownCUDD(manager);
    return (EXIT_SUCCESS);
}


